﻿using System;
using System.Collections.Generic;
using Gameplay.PVE.Utils;
using UnityEngine;
using Yoozoo.Gameplay.RTS;
using Yoozoo.Libs;
using Yoozoo.Managers;

namespace Gameplay.PVE.Survivor
{
    public class TruckWheel : MonoBehaviour
    {
        public Transform leftFront;
        public Transform rightFront;

        public Transform leftBack;

        public Transform rightBack;

        private float L;
        private float outR;

        public float angleSpeed = 0;
        public float moveSpeed = 0;

        public bool needSkip = false;
        public bool canSkip = false;

        private float m_angleSpeed;
        private float targetAngleSpeed;
        private float angleSpeedChangeSpeed = 30f;

        private float targetMoveSpeed;
        private float moveSpeedDecreaseSpeed = 8f;
        private float moveSpeedIncreaseSpeed = 3;

        private float timeScale;
        private float targetTimeScale;
        private float timeScaleIncreaseSpeed = 1f;
        private float timeScaleDecreaseSpeed = 2f;

        private List<Vector3> pathList = new List<Vector3>();

        private int lineId;

        public float simulateStep = 0.2f;
        public Transform debugRoute;

        private GameObject debugRoot;

        private Vector3 anchorOffset;
        
        public void SimulatePathByDebug()
        {
            Initialize();
            if (debugRoute == null)
            {
                return;
            }
            m_angleSpeed = 0;
            targetMoveSpeed = 3;
            if (debugRoot != null)
            {
                GameObject.DestroyImmediate(debugRoot);
            }
            debugRoot = new GameObject("Debug");
            var list = new List<Vector3>();
            for (int i = 0; i < debugRoute.childCount; i++)
            {
                var point = debugRoute.GetChild(i).position;
                list.Add(point);
            }
            SimulateRoute(list);
            for (int i = 0; i < pathList.Count; i++)
            {
                var sphere = GameObject.CreatePrimitive(PrimitiveType.Sphere);
                sphere.transform.position = pathList[i];
                sphere.transform.localScale = Vector3.one * 0.1f;
                sphere.transform.SetParent(debugRoot.transform);
            }
        }

        public MapTruckPath SavePath()
        {
            if (debugRoot == null)
            {
                return null;
            }
            var path = new MapTruckPath();
            var time = 0f;
            moveSpeed = 3;
            var lastPoint = Vector3.zero;
            for (int i = 0; i < debugRoot.transform.childCount; i++)
            {
                var child = debugRoot.transform.GetChild(i);
                var point = child.position;
                var timeDiff = 0f;
                if (lastPoint != Vector3.zero)
                {
                    timeDiff = (Vector3.Distance(lastPoint,child.position) / moveSpeed);
                }
                time += timeDiff;
                lastPoint = point;
                path.points.Add(new MapTruckPathPoint
                {
                    angle = child.eulerAngles.y,
                    pos = child.position,
                    time = time,
                });
            }
            return path;
        }

        public MapTruckPath SimulatePathCircle(MapRoute data, float speed)
        {
            var path = new MapTruckPath();
            var time = 0f;
            var route = data.points;
            var position = route[0];
            var forward = route[1] - route[0];
            var deltaTime = simulateStep;
            moveSpeed = speed;
            var startForward = transform.forward;
            transform.forward = forward;
            path.points.Add(new MapTruckPathPoint
            {
                angle = transform.eulerAngles.y,
                pos = position,
                time = time,
            });
            for (int i = 0; i < route.Count - 1; i++)
            {
                var currentPoint = route[i];
                var nextPoint = route[i + 1];
                var dir = nextPoint - currentPoint;
                var currentForward = transform.forward;
                var angle = 90f - Vector3.Angle(currentForward, dir);
                var radius = Vector3.Distance(currentPoint, nextPoint) / 2f / Mathf.Cos(angle * Mathf.Deg2Rad);
                var needStraight = false;
                if (radius < 0)
                {
                    time += Vector3.Distance(nextPoint, currentPoint) / moveSpeed;
                    path.points.Add(new MapTruckPathPoint
                    {
                        angle = transform.eulerAngles.y,
                        pos = nextPoint,
                        time = time,
                    });
                    continue;
                }
                if (radius > 10)
                {
                    needStraight = true;
                }
                var ratio = 1f;
                if (PveUtils.GetAngle(currentForward, dir) < 0)
                {
                    ratio = -1f;
                }
                
                if (needStraight)
                {
                    var startPos = currentPoint;
                    var angleSpeed = 30 * deltaTime;
                    var c = 0;
                    while (c < 100)
                    {
                        if (Vector3.Angle(transform.forward, dir) <= angleSpeed)
                        {
                            time += deltaTime;
                            transform.position = startPos + transform.forward * moveSpeed * deltaTime;
                            transform.forward = dir;
                            path.points.Add(new MapTruckPathPoint
                            {
                                angle = transform.eulerAngles.y,
                                pos = transform.position,
                                time = time,
                            });
                            time += Vector3.Distance(transform.position,nextPoint) / moveSpeed;
                            transform.forward = dir;
                            path.points.Add(new MapTruckPathPoint
                            {
                                angle = transform.eulerAngles.y,
                                pos = nextPoint,
                                time = time,
                            });
                            break;
                        }
                        time += deltaTime;
                        transform.forward = Quaternion.AngleAxis(angleSpeed * ratio, Vector3.up) * transform.forward;
                        startPos = startPos + transform.forward * moveSpeed * deltaTime;
                        path.points.Add(new MapTruckPathPoint
                        {
                            angle = transform.eulerAngles.y,
                            pos = startPos,
                            time = time,
                        });
                        c++;
                    }
                }
                else
                {
                    var center = currentPoint +
                                 Quaternion.AngleAxis(ratio * 90, Vector3.up) * currentForward.normalized * radius;
                    var angleTotal = (180 - angle * 2f);
                    var angleDiff = GetCircleCenterAngle(moveSpeed * deltaTime, radius);
                    var needContinue = false;
                    var sForward = transform.forward;
                    var startAngle = transform.eulerAngles.y;
                    for (var j = angleDiff; j < angleTotal; j += angleDiff)
                    {
                        time = time + deltaTime;
                        var pos = center + Quaternion.AngleAxis(ratio * j, Vector3.up) * (currentPoint - center);
                        transform.eulerAngles = new Vector3(0, startAngle + ratio * j, 0);
                        path.points.Add(new MapTruckPathPoint
                        {
                            angle = transform.eulerAngles.y,
                            pos = pos,
                            time = time,
                        });
                    }
                    time = time + deltaTime;
                    transform.eulerAngles = new Vector3(0, startAngle + ratio * angleTotal, 0);
                    path.points.Add(new MapTruckPathPoint
                    {
                        angle = transform.eulerAngles.y,
                        pos = nextPoint,
                        time = time,
                    });
                }
            }

            transform.forward = startForward;
            ShowDebug(path);
            return path;
        }
        
        public MapTruckPath SimulatePath(MapRoute data,float speed)
        {
            var path = new MapTruckPath();
            var time = 0f;
            var route = data.points;
            var startForward = transform.forward;
            var position = route[0];
            var forward = route[1] - route[0];
            transform.position = position;
            transform.forward = forward;
            Initialize();
            moveSpeed = speed;
            targetMoveSpeed = speed;
            var deltaTime = simulateStep;
            path.points.Add(new MapTruckPathPoint
            {
                angle = transform.eulerAngles.y,
                pos = position,
                time = time,
            });
            for (int i = 0; i < route.Count - 1; i++)
            {
                canSkip = false;
                var count = 0;
                while (count < 100)
                {
                    time += deltaTime;
                    var targetPosition = route[i + 1];
                    var diff = targetPosition - position;
                    var nextTargetPosition = BattleConstValue.DEFAULT_POINT;
                    /*if (i < route.Count - 2)
                    {
                        nextTargetPosition = route[i + 2];
                    }*/
                    var lastAngleSpeed = m_angleSpeed;
                    var angleChanged = SetTargetPosition(targetPosition,nextTargetPosition,deltaTime);
                    UpdateSpeed(deltaTime);
                    var frameSpeed = moveSpeed * deltaTime;
                    var needRecord = true;
                    if (m_angleSpeed == 0)
                    {
                        if (lastAngleSpeed != m_angleSpeed || angleChanged)
                        {
                            transform.forward = this.forward;
                            /*path.points.Add(new MapTruckPathPoint
                            {
                                time = time,
                                pos = new Vector2(this.position.x, this.position.z),
                                angle = transform.eulerAngles.y,
                            });*/
                            var dt = Mathf.Abs(lastAngleSpeed - m_angleSpeed) / angleSpeedChangeSpeed;
                            if (dt > 0)
                            {
                                this.position = this.position + dt * transform.forward * frameSpeed;
                                transform.forward = (targetPosition - this.position).normalized;
                                transform.position = this.position;
                                if (Vector3.Distance(targetPosition, this.position) < frameSpeed)
                                {
                                
                                }
                                //BattleDebug.LogError(targetPosition + " " + this.position + " " + transform.eulerAngles.y);
                                time = time + dt;
                                path.points.Add(new MapTruckPathPoint
                                {
                                    time = time,
                                    pos = this.position,
                                    angle = transform.eulerAngles.y,
                                });
                            }
                        }
                        transform.forward = forward;
                        //BattleDebug.LogError(targetPosition + " " + this.position + " " + transform.eulerAngles.y);
                        this.forward = (targetPosition - this.position).normalized;
                        time += Vector3.Distance(targetPosition, this.position) / moveSpeed;
                        this.position = targetPosition;
                    }
                    else
                    {
                        UpdateRotation(deltaTime);
                    }

                    position = this.position;
                    forward = this.forward;
                    transform.position = position;
                    transform.forward = forward;
                    path.points.Add(new MapTruckPathPoint
                    {
                        time = time,
                        pos = position,
                        angle = transform.eulerAngles.y,
                    });
                    //UnityEngine.Debug.LogError(position + " " + targetAngleSpeed + " " + angleSpeed + " " + moveSpeed + " " + targetMoveSpeed);
                    if (diff.magnitude <= frameSpeed || needSkip || m_angleSpeed == 0)
                    {
                        //BattleDebug.LogError(needSkip + " " + angleSpeed);
                        break;
                    }
                    count++;
                }
            }
            path.points.Add(new MapTruckPathPoint
            {
                angle = transform.eulerAngles.y,
                pos = route[route.Count - 1],
                time = time,
            });
            /*for (int i = 0; i < path.points.Count - 1; i++)
            {
                var f = new Vector3(path.points[i + 1].pos.x,0,path.points[i + 1].pos.y) - new Vector3(path.points[i].pos.x,0,path.points[i].pos.y);
                path.points[i + 1].angle = PveUtils.GetAngle(Vector3.forward,f);
            }*/
            transform.localPosition = Vector3.zero;
            transform.forward = startForward;
            this.position = transform.position;
            this.forward = startForward;
            ShowDebug(path);
            return path;
        }

        public void ShowDebug(MapTruckPath path)
        {
            if (debugRoot != null)
            {
                GameObject.DestroyImmediate(debugRoot);
            }
            debugRoot = new GameObject("Debug");
            for (int i = 0; i < path.points.Count; i++)
            {
                var cube = GameObject.CreatePrimitive(PrimitiveType.Cube);
                cube.transform.position = path.points[i].pos;
                cube.transform.localScale = new Vector3(0.1f, 0.1f, 0.2f);
                cube.transform.SetParent(debugRoot.transform);
                cube.transform.localEulerAngles = new Vector3(0,path.points[i].angle,0);
                cube.name = i + "_" + path.points[i].time;
            }
        }
        
        public void SimulateRoute(List<Vector3> route)
        {
            var startForward = transform.forward;
            moveSpeed = targetMoveSpeed;
            pathList.Clear();
            var position = route[0];
            var forward = route[1] - route[0];
            transform.position = position;
            transform.forward = forward;
            var deltaTime = simulateStep;
            pathList.Add(position);
            for (int i = 0; i < route.Count - 1; i++)
            {
                canSkip = false;
                var count = 0;
                while (count < 100)
                {
                    var targetPosition = route[i + 1];
                    var diff = targetPosition - position;
                    var nextTargetPosition = BattleConstValue.DEFAULT_POINT;
                    if (i < route.Count - 2)
                    {
                        nextTargetPosition = route[i + 2];
                    }
                    SetTargetPosition(targetPosition,nextTargetPosition,deltaTime);
                    UpdateSpeed(deltaTime);
                    var frameSpeed = moveSpeed * deltaTime;
                    if (m_angleSpeed == 0)
                    {
                        this.position = targetPosition;
                    }
                    else
                    {
                        UpdateRotation(deltaTime);
                    }

                    position = this.position;
                    forward = this.forward;
                    pathList.Add(position);
                    //UnityEngine.Debug.LogError(position + " " + targetAngleSpeed + " " + angleSpeed + " " + moveSpeed + " " + targetMoveSpeed);
                    transform.position = position;
                    transform.forward = forward;
                    if (diff.magnitude <= frameSpeed || needSkip || m_angleSpeed == 0)
                    {
                        //BattleDebug.LogError(needSkip + " " + angleSpeed);
                        break;
                    }
                    count++;
                }
            }
            pathList.Add(route[route.Count - 1]);
            transform.localPosition = Vector3.zero;
            transform.forward = startForward;
            this.position = transform.position;
            this.forward = startForward;
        }

        public void ShowLine()
        {
            lineId = GuideLineManager.Instance.ShowLine(pathList);
        }
        public void SetTargetSpeed(float targetSpeed)
        {
            this.targetMoveSpeed = targetSpeed;
        }
        
        public bool SetTargetPosition(Vector3 targetPosition,Vector3 nextTargetPosition,float deltaTime = 0)
        {
            if (deltaTime == 0)
            {
                deltaTime = Time.deltaTime;
            }
            needSkip = false;
            var angle = PveUtils.GetAngle(targetPosition - transform.position, transform.forward);
            var minAngle = deltaTime * angleSpeed;
            if (Mathf.Abs(angle) < minAngle)
            {
                targetAngleSpeed = 0;
                m_angleSpeed = 0;
                return minAngle > 0.1f;
            }
            else if (angle > 0)
            {
                targetAngleSpeed = angleSpeed;
            }
            else
            {
                targetAngleSpeed = -angleSpeed;
            }
            return true;
            //BattleDebug.LogError(angle + " " + nextAngle + " " + targetPosition);
        }

        public Vector3 position;

        public Vector3 forward = Vector3.forward;

        public bool isDebug;

        private void Update()
        {
            if (isDebug)
            {
                Initialize();
                UpdateRotation();
                transform.position = position;
                transform.forward = forward;
            }
        }

        public void UpdateSpeed(float deltaTime = 0)
        {
            BattleHelper.UpdateValueChange(ref m_angleSpeed,ref targetAngleSpeed,angleSpeedChangeSpeed,deltaTime);
            if (moveSpeed > targetMoveSpeed)
            {
                BattleHelper.UpdateValueChange(ref moveSpeed, ref targetMoveSpeed, moveSpeedDecreaseSpeed,deltaTime);
            }
            else if (moveSpeed < targetMoveSpeed)
            {
                BattleHelper.UpdateValueChange(ref moveSpeed, ref targetMoveSpeed, moveSpeedIncreaseSpeed,deltaTime);
            }
            
            //BattleDebug.LogError("AngleSpeed = " + angleSpeed);
        }
        
        public void UpdateRotation(float deltaTime = 0)
        {
            if (deltaTime == 0)
            {
                deltaTime = Time.deltaTime;
            }
            var angleDiff = 0f;
            var newPosition = GetPositionByRotation(m_angleSpeed,ref angleDiff,deltaTime);
            position = newPosition;
            forward = Quaternion.AngleAxis(angleDiff, Vector3.up) * forward; 
        }

        public void Initialize()
        {
            L = (leftBack.position - leftFront.position).magnitude;
            position = transform.position;
            forward = transform.forward;
            m_angleSpeed = 0;
            moveSpeed = 0;
            targetAngleSpeed = 0;
            targetMoveSpeed = 0;
            /*var originForward = transform.forward;
            transform.localEulerAngles = Vector3.zero;
            var anchorPoint = (leftFront.position + rightFront.position) / 2;
            anchorOffset = anchorPoint - transform.position;
            BattleDebug.LogError(anchorOffset);
            transform.forward = originForward;*/
        }

        public Vector3 GetPositionByRotation(float angle,ref float angleDiff,float deltaTime = 0)
        {
            if (deltaTime == 0)
            {
                deltaTime = Time.deltaTime;
            }
            var circleCenter = GetCirclePoint(angle);
            //BattleDebug.LogError(circleCenter);
            angleDiff = GetCircleCenterAngle(moveSpeed * deltaTime, outR);
            if (angle > 0)
            {
                angleDiff = -angleDiff;
            }
            return circleCenter + Quaternion.AngleAxis(angleDiff, Vector3.up) * (transform.position - circleCenter);
            /*var r = Vector3.Distance(leftFront.position, circleCenter);
            var leftFrontPosition = leftFront.position;
            var leftBackPosition = leftBack.position;
            var rightFrontPosition = rightFront.position;
            var rightBackPosition = rightBack.position;

            var newLeftFrontPosition =
                circleCenter + Quaternion.AngleAxis(angle, Vector3.up) * (leftFrontPosition - circleCenter);
            var newRightFrontPosition =
                circleCenter + Quaternion.AngleAxis(angle, Vector3.up) * (rightFrontPosition - circleCenter);
            var newLeftBackPosition =
                circleCenter + Quaternion.AngleAxis(angle, Vector3.up) * (leftBackPosition - circleCenter);
            var newRightBackPosition =
                circleCenter + Quaternion.AngleAxis(angle, Vector3.up) * (rightBackPosition - circleCenter);*/
            return circleCenter + Quaternion.AngleAxis(angle, Vector3.up) * (transform.position - circleCenter);
        }
        
        private Vector3 GetCirclePoint(float angle)
        {
            Debug.Log("GetCirclePoint:" + angle);
            float absAngle = Mathf.Abs(angle);
            float absAngle2 = (absAngle * (Mathf.PI)) / 180f;
            float r = L / Mathf.Sin(absAngle2);
            outR = r;
            float length = r * Mathf.Cos(absAngle2);
            //Debug.Log("转弯半径:" + r + ",length:" + length);
            Vector3 result = Vector3.zero;
            Vector3 point1 = leftBack.position; //左后轮
            point1 = new Vector3(point1.x, 0, point1.z);
            Vector3 point2 = rightBack.transform.position; //右后轮
            point2 = new Vector3(point2.x, 0, point2.z);
            Vector3 right = (point2 - point1).normalized;
            Vector3 left = (point1 - point2).normalized;
            if (angle < 0) //右转
            {
                result = point1 + right * length;
            }
            else if (angle > 0)
            {
                result = point2 + left * length;
            }

            return result;
        }
        
        //已知半径和弧长，求圆心角
        private float GetCircleCenterAngle(float l, float r)
        {
            return l * 360 / (2 * Mathf.PI * r);
        }
        
        //已知圆上的点、圆心、半径，求角度
        private float GetStartAngle(Vector3 wheelPoint, Vector3 centerPoint, float r)
        {
            //Debug.LogError("GetStartAngle:" + r);
            float sinVal = Mathf.Min(Mathf.Abs(wheelPoint.z - centerPoint.z) / r, 1f); ;
            if (Mathf.Approximately(wheelPoint.x, centerPoint.x) && Mathf.Approximately(sinVal, 1f))
            {
                return 90f;
            }
            //Debug.Log("GetStartAngle:" + sinVal);
            float angleVal = (float)System.Math.Asin(sinVal);
            float result = angleVal / Mathf.PI * 180f;
            if(wheelPoint.x > centerPoint.x)
            {
                if (wheelPoint.z >= centerPoint.z)
                {
                    return result;
                }else
                {
                    return -result;
                }
            }
            if(wheelPoint.x <centerPoint.x)
            {
                if(wheelPoint.z >= centerPoint.z)
                {
                    return 180f - result;
                }else
                {
                    return 180f + result;
                }
            }
            //Debug.Log("GetStartAngle:" + result);
            return result;
        }
        
        
        //通过圆心、半径、角度计算圆上的坐标点
        private Vector3 GetNextPointByAngle(Vector3 centerPoint,float radius, float startAngle, float diffAngle)
        {
            //Debug.Log("GetNextPointByAngle:" + centerPoint + "," + radius + "," + startAngle + "," + diffAngle);
            float angle = startAngle + diffAngle;
            float x1 = centerPoint.x + radius * Mathf.Cos(angle * Mathf.PI / 180f);
            float y1 = centerPoint.z + radius * Mathf.Sin(angle * Mathf.PI / 180f);
            Vector3 pos = new Vector3(x1, centerPoint.y, y1);
            return pos;
        }

        public void SetTargetTimeScale(float timeScale)
        {
            targetTimeScale = timeScale;
        }
        
        private float startTime;
        private int currentIndex;
        private MapTruckPath path;
        private float currentTime;
        private List<int> lines = new List<int>();
        
        public void StartMoveByPath(MapTruckPath path)
        {
            startTime = TimeManager.time;
            currentTime = startTime;
            this.path = path;
            position = path.points[0].pos;
            forward = Quaternion.Euler(0, path.points[0].angle, 0) * Vector3.forward;
            currentIndex = 0;
            for (int i = 0; i < path.points.Count - 1; i++)
            {
                //var angle = path.points[i].angle;
                //var offset = Quaternion.AngleAxis(angle, Vector3.up) * anchorOffset / 2;
                var lineId = GuideLineManager.Instance.ShowLine(path.points[i].pos, path.points[i + 1].pos);
                lines.Add(lineId);
            }
        }

        private void UpdateTimeScale()
        {
            if (targetTimeScale > timeScale)
            {
                BattleHelper.UpdateValueChange(ref timeScale, ref targetTimeScale, timeScaleIncreaseSpeed);
            }
            else if (targetTimeScale < timeScale)
            {
                BattleHelper.UpdateValueChange(ref timeScale, ref targetTimeScale, timeScaleDecreaseSpeed);
            }
        }

        private Vector3 targetForward;

        private void UpdateForward()
        {
            if (targetForward != forward)
            {
                var angle = PveUtils.GetAngle(forward, targetForward);
                var angleThisFrame = m_angleSpeed * Time.deltaTime;
                if (Mathf.Abs(angle) < angleThisFrame)
                {
                    forward = targetForward;
                }
                else
                {
                    if (angle < 0)
                    {
                        angleThisFrame = -angleThisFrame;
                    }
                    forward = Quaternion.AngleAxis(angleThisFrame, Vector3.up) * forward;
                }
            }
        }
        
        public void MoveByPath()
        {
            transform.localPosition = Vector3.zero;
            m_angleSpeed = 30;
            UpdateTimeScale();
            currentTime += Time.deltaTime * TimeManager.logicTimeScale * timeScale;
            var time = currentTime;
            var fromStartTime = time - startTime;
            while (path.points.Count > currentIndex + 1)
            {
                var point = path.points[currentIndex];
                var nextPoint = path.points[currentIndex + 1];
                //BattleDebug.LogError(fromStartTime + " " + point.angle + " " + nextPoint.angle + " " + currentIndex);
                if (fromStartTime < nextPoint.time)
                {
                    var percent = (fromStartTime - point.time) / (nextPoint.time - point.time);
                    var position = Vector3.Lerp(point.pos, nextPoint.pos, percent);
                    var angle = Mathf.LerpAngle(point.angle, nextPoint.angle, percent);
                    this.position = position;
                    this.forward = Quaternion.Euler(0, angle, 0) * Vector3.forward;
                    //targetForward = new Vector3(nextPoint.pos.x, 0, nextPoint.pos.y) - new Vector3(point.pos.x,0,point.pos.y);
                    break;
                }
                else
                {
                    if (lines.Count > currentIndex)
                    {
                        GuideLineManager.Instance.HideSingleLine(lines[currentIndex]);
                    }
                    currentIndex++;
                }
            }
            //UpdateForward();
        }
    }
}
